﻿using System;
using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Configuration;
using System.IO.Ports;
using System.Linq;
using System.Net;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace ayxjDllClass
{


    public class AxisControl
    {
        static SerialPort serialPort;
        static SerialPort serialPortJH;
        static SerialPort serialPortF;
        static Thread snewPortSend;
        static Thread snewPortSendJH;
        static Thread snewPortFsend;

        static CanChannel canPort;
        static Thread canCommunication;
        static uint canIndex = 0;

        public static bool isCan = true;
        public static bool Model2 = true;
        static UdpClass udp;
        static IPEndPoint comServer = new IPEndPoint(IPAddress.Parse("192.168.0.8"), 1024);
        static int ComServerEncodeLocalPort = 5567;

        #region 报文封装

        public static byte[] CRC16(byte[] data)
        {
            int len = data.Length;

            if (len > 0)
            {
                ushort crc = 0xFFFF;

                for (int i = 0; i < len; i++)
                {
                    crc = (ushort)(crc ^ (data[i]));
                    for (int j = 0; j < 8; j++)
                    {
                        crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1);
                    }
                }
                byte hi = (byte)((crc & 0xFF00) >> 8); //高位置
                byte lo = (byte)(crc & 0x00FF); //低位置

                return new byte[] { hi, lo };
            }
            return new byte[] { 0, 0 };
        }
        /// <summary>
        /// 写入单个寄存器方法 06
        /// </summary>
        /// <param name="Sadd">设备地址</param>
        /// <param name="Jadd">寄存器地址</param>
        /// <param name="data">写入的数据</param>
        /// <returns></returns>
        public static byte[] addreData(byte Sadd, Int16 Jadd, double data)
        {
            byte[] datas = new byte[] { 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            datas[0] = Sadd;
            byte[] Jadds = BitConverter.GetBytes(Jadd);
            Array.Reverse(Jadds);
            Jadds.CopyTo(datas, 2);
            byte[] temperature = BitConverter.GetBytes(Convert.ToInt16(data));
            Array.Reverse(temperature);
            temperature.CopyTo(datas, 4);
            // 使用Array.Copy方法复制原始数组的前六位到新数组  
            byte[] dee = new byte[6];
            Array.Copy(datas, 0, dee, 0, 6);
            byte[] crc3 = CRC16(dee);
            Array.Reverse(crc3);
            //byte[] crc2 = BitConverter.GetBytes(crc3); // 将 ushort 转换为 byte 数组
            crc3.CopyTo(datas, 6);
            return datas;
        }
        /// <summary>
        /// 读取寄存器方法 03
        /// </summary>
        /// <param name="Sadd">设备地址</param>
        /// <param name="Jadd">寄存器地址</param>
        /// <param name="data">写入的数据</param>
        /// <returns></returns>
        public static byte[] addreDataRead(byte Sadd, Int16 Jadd, double data)
        {
            byte[] datas = new byte[] { 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            datas[0] = Sadd;
            byte[] Jadds = BitConverter.GetBytes(Jadd);
            Array.Reverse(Jadds);
            Jadds.CopyTo(datas, 2);
            byte[] temperature = BitConverter.GetBytes(Convert.ToInt16(data));
            Array.Reverse(temperature);
            temperature.CopyTo(datas, 4);
            // 使用Array.Copy方法复制原始数组的前六位到新数组  
            byte[] dee = new byte[6];
            Array.Copy(datas, 0, dee, 0, 6);
            byte[] crc3 = CRC16(dee);
            Array.Reverse(crc3);
            //byte[] crc2 = BitConverter.GetBytes(crc3); // 将 ushort 转换为 byte 数组
            crc3.CopyTo(datas, 6);
            return datas;
        }

        #endregion

        #region 串口通讯通道

        //队列
        static ConcurrentQueue<byte[]> queue = new ConcurrentQueue<byte[]>();
        static ConcurrentQueue<byte[]> queueJH = new ConcurrentQueue<byte[]>();


        public static ConcurrentQueue<VCI_CAN_OBJ> canQueue = new ConcurrentQueue<VCI_CAN_OBJ>();

        public static void DriverSend()//读取方位俯仰驱动器限位报警等状态
        {
            //方位
            byte[] dataDI = addreDataRead(0x01, 923, 8);//限位状态
            byte[] dataAL = addreDataRead(0x01, 940, 7);//警报状态
                                                        // byte[] dataCurrent = addreDataRead(0x01, 900, 1);//电流
            byte[] dataSpeed = addreDataRead(0x01, 904, 2);//速度
            //俯仰
            byte[] FYdataDI = addreDataRead(0x02, 923, 8);//限位状态
            byte[] FYdataAL = addreDataRead(0x02, 940, 7);//警报状态
                                                          // byte[] FYdataCurrent = addreDataRead(0x02, 900, 1);//电流
            byte[] FYdataSpeed = addreDataRead(0x02, 904, 2);//速度
            while (true)
            {
                if (serialPort == null || !serialPort.IsOpen) { break; }
                if (!IsConnect && queue.Count == 0) { serialPort.Close(); serialPort.Dispose(); break; }
                try
                {
                    if (queue.Count == 0)
                    {
                        queue.Enqueue(dataSpeed);
                        queue.Enqueue(dataDI);
                        queue.Enqueue(dataAL);
                        // queue.Enqueue(dataCurrent);
                        queue.Enqueue(FYdataSpeed);
                        queue.Enqueue(FYdataDI);
                        queue.Enqueue(FYdataAL);
                        //queue.Enqueue(FYdataCurrent);
                    }
                    if (queue.Count == 0) continue;
                    queue.TryDequeue(out byte[] sead);
                    serialPort.Write(sead, 0, sead.Length);
                    Thread.Sleep(50);
                    int zijie = serialPort.BytesToRead;
                    if (zijie > 0)
                    {
                        byte[] res = new byte[zijie];
                        serialPort.Read(res, 0, zijie);
                        if (res.Length >= 8)
                        {
                            if (res[0] == 0x01)
                            {
                                #region
                                if (res.Length > 20)
                                {
                                    ErrStr[0] = "NONE";
                                    if (res[1] == 0x03 && res[2] == 0x10)
                                    {
                                        byte[] array = new byte[19];
                                        Array.Copy(res, 0, array, 0, 19);
                                        byte[] crc = CRC16(array);
                                        Array.Reverse(crc);
                                        if (crc[0] == res[19] && crc[1] == res[20])
                                        {
                                            if (res[4] == 04)
                                            {
                                                boolRead3[0] = true;//顺限
                                            }
                                            else if (res[4] == 08)
                                            {
                                                boolRead6[0] = true;//逆限
                                            }
                                            else
                                            {
                                                boolRead3[0] = boolRead6[0] = false;//正常
                                            }
                                        }

                                    }
                                    else if (res[1] == 0x03 && res[2] == 0x0E)
                                    {
                                        byte[] array = new byte[17];
                                        Array.Copy(res, 0, array, 0, 17);
                                        byte[] crc = CRC16(array);
                                        Array.Reverse(crc);
                                        if (crc[0] == res[17] && crc[1] == res[18])
                                        {
                                            if (res[5] != 0 && res[6] != 0)
                                            {
                                                boolOvervoltage[0] = true;

                                            }
                                            if (res[7] != 0 && res[8] != 0)
                                            {
                                                boolUndervoltage[0] = true;

                                            }
                                            if (res[13] != 0 && res[14] != 0)
                                            {
                                                boolOverload[0] = true;

                                            }
                                            if (res[15] != 0 && res[16] != 0)
                                            {
                                                boolSpeeding[0] = true;
                                            }
                                        }

                                    }
                                }
                                else if (res.Length == 7 && res[1] == 0x03 && res[2] == 0x02)
                                {
                                    byte[] array = new byte[5];
                                    Array.Copy(res, 0, array, 0, 5);
                                    byte[] crc = CRC16(array);
                                    Array.Reverse(crc);
                                    if (crc[0] == res[5] && crc[1] == res[6])
                                    {
                                        byte[] current = new byte[2];
                                        Array.Copy(res, 3, current, 0, 2);
                                    }
                                }
                                else if (res.Length == 9 && res[1] == 0x03 && res[2] == 0x04)
                                {
                                    byte[] array = new byte[7];
                                    Array.Copy(res, 0, array, 0, 7);
                                    byte[] crc = CRC16(array);
                                    Array.Reverse(crc);
                                    if (crc[0] == res[7] && crc[1] == res[8])
                                    {
                                        byte[] speed = new byte[4];
                                        Array.Copy(res, 3, speed, 0, 4);
                                        byte aa = speed[0];
                                        byte bb = speed[1];
                                        speed[0] = speed[2];
                                        speed[1] = speed[3];
                                        speed[2] = aa;
                                        speed[3] = bb;
                                        Array.Reverse(speed);
                                        SpeedNum[0] = (double)((BitConverter.ToInt32(speed, 0)) / sb[0]);
                                    }
                                }
                                #endregion
                            }
                            else if (res[0] == 0x02)
                            {
                                #region
                                if (res.Length > 20)
                                {
                                    ErrStr[1] = "NONE";
                                    if (res[1] == 0x03 && res[2] == 0x10)//限位
                                    {
                                        byte[] array = new byte[19];
                                        Array.Copy(res, 0, array, 0, 19);
                                        byte[] crc = CRC16(array);
                                        Array.Reverse(crc);
                                        if (crc[0] == res[19] && crc[1] == res[20])
                                        {
                                            if (res[4] == 04)
                                            {
                                                boolRead3[1] = true;//顺限
                                            }
                                            else if (res[4] == 08)
                                            {
                                                boolRead6[1] = true;//逆限
                                            }
                                            else
                                            {
                                                boolRead3[1] = boolRead6[1] = false;//正常
                                            }
                                        }

                                    }
                                    else if (res[1] == 0x03 && res[2] == 0x0E)
                                    {
                                        byte[] array = new byte[17];
                                        Array.Copy(res, 0, array, 0, 17);
                                        byte[] crc = CRC16(array);
                                        Array.Reverse(crc);
                                        if (crc[0] == res[17] && crc[1] == res[18])
                                        {
                                            if (res[5] != 0 && res[6] != 0)
                                            {
                                                boolOvervoltage[1] = true;
                                            }
                                            if (res[7] != 0 && res[8] != 0)
                                            {
                                                boolUndervoltage[1] = true;

                                            }
                                            if (res[13] != 0 && res[14] != 0)
                                            {
                                                boolOverload[1] = true;

                                            }
                                            if (res[15] != 0 && res[16] != 0)
                                            {
                                                boolSpeeding[1] = true;
                                            }
                                        }

                                    }
                                }
                                else if (res.Length == 9 && res[1] == 0x03 && res[2] == 0x04)//速度
                                {
                                    byte[] array = new byte[7];
                                    Array.Copy(res, 0, array, 0, 7);
                                    byte[] crc = CRC16(array);
                                    Array.Reverse(crc);
                                    if (crc[0] == res[7] && crc[1] == res[8])
                                    {
                                        byte[] speed = new byte[4];
                                        Array.Copy(res, 3, speed, 0, 4);
                                        byte aa = speed[0];
                                        byte bb = speed[1];
                                        speed[0] = speed[2];
                                        speed[1] = speed[3];
                                        speed[2] = aa;
                                        speed[3] = bb;
                                        Array.Reverse(speed);
                                        SpeedNum[1] = (double)((BitConverter.ToInt32(speed, 0)) / sb[1]);
                                    }
                                }
                                #endregion
                            }
                        }
                        else { continue; }
                    }
                }
                catch
                {
                    Console.WriteLine("方位俯仰异常");
                }
            }
        }

        public static void DriverSendJH()//读取极化驱动器限位报警等状态
        {
            //极化
            byte[] JHdataDI = addreDataRead(0x03, 44, 2);//限位状态
            byte[] JHdataAL = addreDataRead(0x03, 51, 1);//警报状态
                                                         // byte[] JHcurrent = addreDataRead(0x03, 33, 1);//电流
            byte[] JHspeed = addreDataRead(0x03, 34, 1);//速度
            while (true)
            {
                if (!serialPortJH.IsOpen) { break; }
                if (!IsConnect && queueJH.Count == 0) { serialPortJH.Close(); serialPortJH.Dispose(); break; }
                try
                {
                    if (queueJH.Count == 0)
                    {
                        queueJH.Enqueue(JHdataDI);
                        queueJH.Enqueue(JHdataAL);
                        //queueJH.Enqueue(JHcurrent);
                        queueJH.Enqueue(JHspeed);
                    }
                    queueJH.TryDequeue(out byte[] sead);
                    serialPortJH.Write(sead, 0, sead.Length);
                    Thread.Sleep(50);
                    int zijie = serialPortJH.BytesToRead;
                    if (zijie > 0)
                    {
                        byte[] res = new byte[zijie];
                        serialPortJH.Read(res, 0, zijie);
                        if (res.Length >= 7)
                        {
                            ErrStr[2] = "NONE";
                            if (sead == JHdataDI)//限位
                            {
                                if (res[1] == 0x03 && res[2] == 0x04)
                                {
                                    byte[] array = new byte[7];
                                    Array.Copy(res, 0, array, 0, 7);
                                    byte[] crc = CRC16(array);
                                    Array.Reverse(crc);
                                    if (crc[0] == res[7] && crc[1] == res[8])
                                    {
                                        if (res[4] == 00) { boolRead3[2] = true; }//顺限
                                        else if (res[6] == 00) { boolRead6[2] = true; }//逆限
                                        else { boolRead3[2] = boolRead6[2] = false; }//正常
                                    }
                                }
                            }
                            else if (sead == JHdataAL)//警告
                            {
                                if (res[1] == 0x03 && res[2] == 0x02)
                                {
                                    byte[] array = new byte[5];
                                    Array.Copy(res, 0, array, 0, 5);
                                    byte[] crc = CRC16(array);
                                    Array.Reverse(crc);
                                    if (crc[0] == res[5] && crc[1] == res[6])
                                    {
                                        switch (res[4])
                                        {
                                            case 0:
                                                boolOvervoltage[2] = true;//过压
                                                boolOverload[2] = true;//过载
                                                boolUndervoltage[2] = true;//欠压
                                                boolSpeeding[2] = true;//超速
                                                break;
                                            default:
                                                break;
                                        }
                                    }
                                }
                            }
                            else if (sead == JHspeed)
                            {
                                if (res[1] == 0x03 && res[2] == 0x02)
                                {
                                    //byte[] array = new byte[5];
                                    //Array.Copy(res, 0, array, 0, 5);
                                    //byte[] crc = CRC16(array);
                                    //Array.Reverse(crc);
                                    //if (crc[0] == res[5] && crc[1] == res[6])
                                    //{
                                    //    byte[] sudu = new byte[2];
                                    //    Array.Copy(res, 3, sudu, 0, 2);
                                    //    Array.Reverse(sudu);
                                    //    SpeedNum[2] = BitConverter.ToInt16(sudu,0) /sb[2];
                                    //}

                                }
                            }
                            else
                            {

                            }
                        }
                    }
                }
                catch { Console.WriteLine("极化异常"); }
            }
        }

        public static void serial1()//方位俯仰极化编码器信息读取
        {
            int ti = 20;
            byte[] serialPort1Buffer = new byte[9];//声明一个临时数组存储当前的串口数据
            byte[] serialPort1Data = new byte[4];
            byte[] id = new byte[3] { 0x11, 0x12, 0x13 };
            while (true)
            {
                if (!serialPortF.IsOpen) { break; }
                if (!IsConnect) { serialPortF.Close(); serialPortF.Dispose(); break; }
                try
                {
                    for (int i = 0; i < 3; i++)
                    {
                        byte[] data = addreDataRead(id[i], 0, 2);
                        serialPortF.Write(data, 0, data.Length);
                        Thread.Sleep(ti);
                        if (serialPortF.BytesToRead > 0)
                        {
                            serialPortF.Read(serialPort1Buffer, 0, 9);
                            if (serialPort1Buffer[0] == id[i] && serialPort1Buffer[1] == 0x03 && serialPort1Buffer[2] == 0x04 && dataVerification.CRC16_MODBUS(serialPort1Buffer, 0, 7)[1] == serialPort1Buffer[8])
                            {
                                serialPort1Data[0] = serialPort1Buffer[3];
                                serialPort1Data[1] = serialPort1Buffer[4];
                                serialPort1Data[2] = serialPort1Buffer[5];
                                serialPort1Data[3] = serialPort1Buffer[6];
                                if (BitConverter.IsLittleEndian) { Array.Reverse(serialPort1Data); }
                                double angle = (BitConverter.ToInt32(serialPort1Data, 0) * 360.000 / zmc[i]) + CorrectedNum[i];
                                if (i != 0)
                                {
                                    if (angle > 180) angle -= 360;
                                    if (angle < -180) angle += 360;
                                }
                                Position[i] = angle;

                                if (RLimit[i, 0] > angle && RLimit[i, 1] < angle) { RLimitState[i, 0] = RLimitState[i, 1] = 0; }
                                else
                                {
                                    if (RLimit[i, 0] < angle && RLimitState[i, 0] == 0) { setMove((short)i, 0); RLimitState[i, 0] = 1; }
                                    if (RLimit[i, 1] > angle && RLimitState[i, 1] == 0) { setMove((short)i, 0); RLimitState[i, 1] = 2; }
                                }
                            }
                        }
                    }
                }
                catch
                {
                    Console.WriteLine("编码异常");
                }
            }
        }

        static byte[] encodeId = new byte[3] { 0x11, 0x12, 0x13 };
        public static void serial2()//方位俯仰极化编码器信息读取
        {
            while (true)
            {
                if (udp == null || udp.UdpClient == null) { break; }
                if (!IsConnect) { udp.UdpClient = null; udp = null; break; }
                try
                {
                    for (int i = 0; i < 3; i++)
                    {
                        byte[] data = addreDataRead(encodeId[i], 0, 2);
                        udp.SendMessage(data, comServer);
                        Thread.Sleep(100);
                    }
                }
                catch
                {
                    Console.WriteLine("编码异常");
                }
            }
        }

        static void UdpBmqHandleMessageEvent(byte[] bytes, EndPoint endPoint)
        {
            try
            {
                if (bytes.Length >= 9)
                {
                    int i = encodeId.ToList().IndexOf(bytes[0]);
                    if (bytes[1] == 0x03 && bytes[2] == 0x04 && dataVerification.CRC16_MODBUS(bytes, 0, 7)[1] == bytes[8])
                    {
                        int angle0 = bytes.ToInt32(3, false);
                        double angle = (angle0 * 360.000 / zmc[i]) + CorrectedNum[i];
                        if (i != 0)
                        {
                            if (angle > 180) angle -= 360;
                            if (angle < -180) angle += 360;
                        }
                        Position[i] = angle;
                        if (RLimit[i, 0] > angle && RLimit[i, 1] < angle) { RLimitState[i, 0] = RLimitState[i, 1] = 0; }
                        else
                        {
                            if (RLimit[i, 0] < angle && RLimitState[i, 0] == 0) { setMove((short)i, 0); RLimitState[i, 0] = 1; }
                            if (RLimit[i, 1] > angle && RLimitState[i, 1] == 0) { setMove((short)i, 0); RLimitState[i, 1] = 2; }
                        }
                    }
                }
            }
            catch { }
        }

        /// <summary>
        /// CAN消息接收处理函数
        /// </summary>
        /// <param name="msgList"></param>
        /// <param name="len"></param>
        public static unsafe void HandleCanMessage(VCI_CAN_OBJ[] msgList, uint len)
        {
            for (int i = 0; i < len; i++)
            {
                VCI_CAN_OBJ obj = msgList[i];
                byte[] data = new byte[obj.DataLen];
                fixed (VCI_CAN_OBJ* obj2 = &msgList[i])
                {
                    for (int j = 0; j < data.Length; j++)
                        data[j] = obj2->Data[j];
                }
                CANProtocel.AnalyseCanData(obj.ID, obj.DataLen, data);
            }
        }

        /// <summary>
        /// CAN通讯子线程，负责发送
        /// </summary>
        public static void CanCommunication()
        {
            uint rlt = 0;
            while (true)
            {
                if (!canPort.IsOpen) { break; }
                if (!IsConnect) { canPort.Close(out rlt); canPort = null; break; }
                try
                {
                    if (canQueue.TryDequeue(out VCI_CAN_OBJ data))
                    { 
                        canPort.SendMessage(canIndex, data);
                    }
                    Task.Delay(10);
                }
                catch { }
            }
        }

        static void Init()
        {
            try
            {
                Driver = false;
                DriverJh = false;
                Encoder = false;

                Model2 = ConfigurationManager.AppSettings["Model2"] == "1";
                string comServerIP = ConfigurationManager.AppSettings["ComServerIP"];
                int port = Convert.ToInt32(ConfigurationManager.AppSettings["ComServerEncodeRemotePort"]);
                comServer = new IPEndPoint(IPAddress.Parse(comServerIP), port);

                ComServerEncodeLocalPort = Convert.ToInt32(ConfigurationManager.AppSettings["ComServerEncodeLocalPort"]);

                CANProtocel.EnabledNum = Convert.ToInt32(ConfigurationManager.AppSettings["EnabledNum"]);
            }
            catch { }
        }

        public static bool Driver { get; set; }= false;
        public static bool DriverJh { get; set; } = false;
        public static bool Encoder { get; set; } = false;

        static int[] zmc = new int[3] { 131072, 131072, 131072 };//总脉冲数
        public static bool Connect(string[] adderss, int[] bote, int[] bote3, char[] bote2, int[] bote4)//连接
        {
            Init();
            isCan = Model2;
            try
            {
                DisConnect();
                IsConnect = true;
                //驱动器串口
                if (!isCan)
                {
                    if (serialPort == null || !serialPort.IsOpen)
                    {
                        serialPort = new SerialPort(adderss[0], bote[0], bote2[0] == 'E' ? Parity.Even : Parity.None, bote3[0], bote4[0] == 1 ? StopBits.One : StopBits.None);
                        serialPort.Open();
                        if (snewPortSend == null || snewPortSend.ThreadState == ThreadState.Stopped || snewPortSend.ThreadState == ThreadState.Aborted)
                        {
                            snewPortSend = new Thread(DriverSend) { IsBackground = true };
                            snewPortSend.Start();
                        }
                        Driver = true;
                    }
                }
                else 
                {
                    if (canPort == null)
                    {
                        canPort = new CanChannel();
                        bool cg = canPort.Open(0, out uint rlt);
                        if (cg)
                        {
                            VCI_INIT_CONFIG initConfig = new VCI_INIT_CONFIG()
                            {
                                AccCode = 0,
                                AccMask = 0xffffffff,
                                Filter = 0,
                                Timing0 = 0,
                                Timing1 = 0x14,
                                Mode = 0,
                            };
                            if (canPort.InitDev(ref initConfig, 0, out rlt))
                            {
                                cg = canPort.StartCan(0, HandleCanMessage, out rlt);
                                canIndex = 0;
                            }
                            else if (canPort.InitDev(ref initConfig, 1, out rlt))
                            {
                                cg = canPort.StartCan(1, HandleCanMessage, out rlt);
                                canIndex = 1;
                            }
                            else
                                cg = false;
                            if (cg)
                            {
                                CANProtocel.SetAutoReport(0);
                                CANProtocel.SetAutoReport(1);
                                if (canCommunication == null || canCommunication.ThreadState == ThreadState.Stopped || canCommunication.ThreadState == ThreadState.Aborted)
                                {
                                    canCommunication = new Thread(CanCommunication) { IsBackground = true };
                                    canCommunication.Start();
                                }
                            }
                            Driver = cg;
                        }
                    }
                }

                //极化驱动器
                if (serialPortJH == null || !serialPortJH.IsOpen)
                {
                    //serialPortJH = new SerialPort(adderss[1], bote[1], bote2[1] == 'E' ? Parity.Even : Parity.None, bote3[1], bote4[1] == 1 ? StopBits.One : StopBits.None);
                    //serialPortJH.Open();
                    //snewPortSendJH = new Thread(DriverSendJH) { IsBackground = true };
                    //snewPortSendJH.Start();
                    //DriverJh = true;
                }
                //编码器串口
                if (Model2)
                {
                    if (udp == null)
                    {
                        udp = new UdpClass(ComServerEncodeLocalPort);
                        udp.HandleMessageEvent += UdpBmqHandleMessageEvent;
                        if (snewPortFsend == null || snewPortFsend.ThreadState == ThreadState.Stopped || snewPortFsend.ThreadState == ThreadState.Aborted)
                        {
                            snewPortFsend = new Thread(serial2) { IsBackground = true };
                            snewPortFsend.Start();
                        }
                        Encoder = true;
                    }
                }
                else
                {
                    if (serialPortF == null || !serialPortF.IsOpen)
                    {
                        serialPortF = new SerialPort(adderss[2], bote[2], bote2[2] == 'E' ? Parity.Even : Parity.None, bote3[2], bote4[2] == 1 ? StopBits.One : StopBits.None);
                        serialPortF.Open();
                        snewPortFsend = new Thread(serial1) { IsBackground = true };
                        snewPortFsend.Start();
                        Encoder = true;
                    }
                }
                return true;
            }
            catch { return false; }
        }

        public static bool DisConnect()//断开
        {
            IsConnect = false;
            if (udp != null)
            {
                udp.UdpClient = null;
                udp = null;
            }
            Thread.Sleep(100);
            return true;

            //if (isCan)
            //{
            //    bool cg = canPort.Close(out uint rlt);
            //    IsConnect = false;
            //    return cg;
            //}
            //else
            //{
            //    IsConnect = false;
            //    Thread.Sleep(100);
            //    return true;
            //}
        }

        #endregion


        public static bool setEnable(short Axis, bool Enable)//轴使能
        {
            if (isCan)
            { 
                CANProtocel.SetEnable(Axis, Enable);
                boolRead1[Axis] = Enable;
                return true;
            }
            Axis += 1;
            if (Axis != 3)
            {
                byte[] asd = addreData(Convert.ToByte(Axis), 100, (Enable ? 1 : 0));
                queue.Enqueue(asd);
                boolRead1[Axis - 1] = Enable;
            }
            else
            {
                boolRead1[Axis - 1] = Enable;
            }
            return true;
        }
        public static double[] Speed = new double[count];//速度
        public static bool setSpeed(short Axis, double speed)//轴速度
        {

            switch (Axis)
            {
                case 0: if (speed < 0.001) Speed[Axis] = 0.001; else if (speed > 0.5) Speed[Axis] = 0.5; else Speed[Axis] = speed; break;
                case 1: if (speed < 0.001) Speed[Axis] = 0.001; else if (speed > 0.5) Speed[Axis] = 0.5; else Speed[Axis] = speed; break;
                case 2: if (speed < 0.001) Speed[Axis] = 0.001; else if (speed > 1) Speed[Axis] = 1; else Speed[Axis] = speed; break;
            }

            return true;
        }
        static double[] sb = new double[3] { 41950, 362880, 100 };
        public static bool[] poT = new bool[3] { false, false, false };
        public static bool setMove(short Axis, short Direction)//轴运动，0停止，1顺转，2逆转
        {
            if (isCan)
            {
                // 默认情况下，顺转角度会越来越大
                // 俯仰轴，顺转角度会变小
                if (Axis == 1) 
                {
                    switch (Direction)
                    {
                        case 1: Direction = 2; break;
                        case 2: Direction = 1; break;
                    }
                }
                return CANProtocel.SetMove(Axis, Direction);
            }
            double speed = sb[Axis] * Speed[Axis];
            if (speed > 30000) { speed = 30000; }
            Axis += 1;
            if (Axis != 3)
            {
                switch (Direction)
                {
                    case 0:
                        queue.Enqueue(addreData(Convert.ToByte(Axis), 412, 260)); poT[Axis - 1] = boolRead4[Axis - 1] = boolRead5[Axis - 1] = false;
                        break;
                    case 1:
                        if (RLimitState[Axis - 1, 0] != 1)
                        {
                            queue.Enqueue(addreData(Convert.ToByte(Axis), 114, speed));
                            queue.Enqueue(addreData(Convert.ToByte(Axis), 412, 4));
                            boolRead4[Axis - 1] = true;
                        }
                        break;
                    case 2:
                        if (RLimitState[Axis - 1, 1] != 2)
                        {
                            queue.Enqueue(addreData(Convert.ToByte(Axis), 114, -1 * speed));
                            queue.Enqueue(addreData(Convert.ToByte(Axis), 412, 4));
                            boolRead5[Axis - 1] = true;
                        }
                        break;
                }
            }
            else
            {
                byte[] data = new byte[1];
                switch (Direction)
                {
                    case 0: data = addreData(0x03, 64, 0); poT[Axis - 1] = boolRead4[Axis - 1] = boolRead5[Axis - 1] = false; SpeedNum[2] = 0; break;
                    case 1:
                        if (RLimitState[Axis - 1, 0] != 1)
                        {
                            data = addreData(0x03, 67, speed); boolRead4[Axis - 1] = true; SpeedNum[2] = Speed[2];
                        }
                        break;
                    case 2:
                        if (RLimitState[Axis - 1, 1] != 2)
                        {
                            data = addreData(0x03, 67, -1 * speed); boolRead5[Axis - 1] = true; SpeedNum[2] = -Speed[2];
                        }
                        break;
                }
                queueJH.Enqueue(data);
            }
            return true;
        }

        public static bool setPosition(short Axis, double Pos, double speed)//轴定位
        {
            int L = 0;
            setSpeed(Axis, speed);
            double sp = speed;
            double count = Pos - Position[Axis];
            if (count > 0.01) { setMove(Axis, 1); }
            else if (count < -0.01) { setMove(Axis, 2); }
            poT[Axis] = true;
            Task.Factory.StartNew(() =>
            {
                while (true)
                {
                    if (poT[Axis] == false || Math.Abs(count - Position[Axis]) < 0.03 || (!boolRead4[Axis] && !boolRead5[Axis])) { setMove(Axis, 0); break; }
                    if (L == 0 && Math.Abs(Pos - Position[Axis]) < 3)
                    {
                        if (sp > 0.5) { sp = 0.5; }
                        setSpeed(Axis, sp);
                        count = Pos - Position[Axis];
                        if (count > 0.01) { setMove(Axis, 1); } else if (count < -0.01) { setMove(Axis, 2); }
                        L = 1;
                    }
                    if (L == 1 && Math.Abs(Pos - Position[Axis]) < 1.5)
                    {
                        if (sp > 0.2) { sp = 0.2; }
                        setSpeed(Axis, sp);
                        count = Pos - Position[Axis];
                        if (count > 0.01) { setMove(Axis, 1); } else if (count < -0.01) { setMove(Axis, 2); }
                        L = 2;
                    }
                    if (L == 2 && Math.Abs(Pos - Position[Axis]) < 0.5)
                    {
                        if (sp > 0.05) { sp = 0.05; }
                        setSpeed(Axis, sp);
                        count = Pos - Position[Axis];
                        if (count > 0.01) { setMove(Axis, 1); } else if (count < -0.01) { setMove(Axis, 2); }
                        L = 3;
                    }
                    if (L == 3 && Math.Abs(Pos - Position[Axis]) < 0.2)
                    {
                        if (sp > 0.03) { sp = 0.03; }
                        setSpeed(Axis, sp);
                        count = Pos - Position[Axis];
                        if (count > 0.01) { setMove(Axis, 1); } else if (count < -0.01) { setMove(Axis, 2); }
                        L = 4;
                    }
                    if (L == 4 && Math.Abs(Pos - Position[Axis]) < 0.2)
                    {
                        if (sp > 0.01) { sp = 0.01; }
                        setSpeed(Axis, sp);
                        count = Pos - Position[Axis];
                        if (count > 0.01) { setMove(Axis, 1); } else if (count < -0.01) { setMove(Axis, 2); }
                        L = 5;
                    }
                    Thread.Sleep(5);
                }
            });
            return true;
        }
        static double[] CorrectedNum = new double[count] { 0, 0, 0 };//轴角修正值
        static double[,] RLimit = new double[3, 2];//轴角限位值
        public static int[,] RLimitState = new int[3, 2];//0不限，1首限位，2限位     [上限、下限]
        public static bool setCorrected(short Axis, double num)//轴角修正
        {
            CorrectedNum[Axis] = num;
            return true;
        }
        public static bool setSpeesB(short Axis, double num)//轴角速比
        {
            sb[Axis] = num;
            return true;
        }
        public static bool setRLimit(short Axis, double CW, double CCW)//轴角软限位
        {
            RLimit[Axis, 0] = CW; RLimit[Axis, 1] = CCW;
            return true;
        }



        #region 获取类状态
        const int count = 3;//轴数
        static bool IsConnect = false;
        static double[] Position = new double[count];//位置
        public static double[] SpeedNum = new double[count];//速度
        static float[] Voltage = new float[count];//电压
        static float[] Electriccurrent = new float[count];//电流
        static string[] ErrStr = new string[count] { "异常", "异常", "异常" };//错误码
        static string[] StateStr = new string[count];//状态码
        static bool[] boolRead3 = new bool[count];//顺限位
        public static bool[] boolRead4 = new bool[count];//顺转
        public static bool[] boolRead5 = new bool[count];//逆转
        static bool[] boolRead6 = new bool[count];//逆限位
        static bool[] boolRead1 = new bool[count];//使能
        static bool[] boolOvervoltage = new bool[count];//过压
        static bool[] boolOverload = new bool[count];//过载
        static bool[] boolUndervoltage = new bool[count];//欠压
        static bool[] boolSpeeding = new bool[count];//超速
        public static double getPosition(short Axis)//轴位置
        {
            return Position[Axis];
        }
        public static double getSpeed(short Axis)//轴速度
        {
            return SpeedNum[Axis];
        }
        public static float getVoltage(short Axis)//轴电压
        {
            return Voltage[Axis];
        }
        public static float getElectriccurrent(short Axis)//轴电流
        {
            return Electriccurrent[Axis];
        }
        public static string getErrStr(short Axis)//轴错误码
        {
            return ErrStr[Axis];
        }
        public static string getStateStr(short Axis)//轴状态码
        {
            return StateStr[Axis];
        }
        public static int getLimit(short Axis)//轴限位0不限，1顺限位，2逆限位,3顺逆限位
        {
            if (boolRead3[Axis] && boolRead6[Axis]) return 3;
            else if (!boolRead3[Axis] && boolRead6[Axis]) return 2;
            else if (boolRead3[Axis] && !boolRead6[Axis]) return 1;
            else if (!boolRead3[Axis] && !boolRead6[Axis]) return 0;
            else return -1;
        }
        public static int getState(short Axis)//轴运动状态0停止，1顺转，2逆转
        {
            if (boolRead4[Axis]) return 1;
            if (boolRead5[Axis]) return 2;
            return 0;
        }
        public static bool getEnable(short Axis)//轴使能
        {
            return boolRead1[Axis];
        }
        public static bool getOvervoltage(short Axis)//轴过压
        {
            return boolOvervoltage[Axis];
        }
        public static bool getOverload(short Axis)//轴过载
        {
            return boolOverload[Axis];
        }
        public static bool getUndervoltage(short Axis)//轴欠压
        {
            return boolUndervoltage[Axis];
        }
        public static bool getSpeeding(short Axis)//轴超速
        {
            return boolSpeeding[Axis];
        }
        #endregion 获取类
    }
    public class SerialPortBase
    {
        SerialPort SerialPortObj;
        /// <summary>
        /// 初始化
        /// </summary>
        /// <param name="portName">COM口名称</param>
        /// <param name="baudRate">波特率</param>
        /// <param name="parity">检验位</param>
        /// <param name="dataBits">数据位</param>
        /// <param name="stopBits">停止位</param>
        public void Init(string portName, int baudRate = 115200, Parity parity = Parity.None, int dataBits = 8, StopBits stopBits = StopBits.One)
        {
            SerialPortObj = new SerialPort(portName, baudRate, parity, dataBits, stopBits);
            if (SerialPortObj.IsOpen) { SerialPortObj.Close(); }
            SerialPortObj.Open();
        }

        /// <summary>
        /// 关闭
        /// </summary>
        public void Close()
        {
            if (SerialPortObj != null && SerialPortObj.IsOpen)
            {
                SerialPortObj.Close(); SerialPortObj.Dispose(); SerialPortObj = null;
            }
        }

        public int SendData(out byte[] Re, byte[] Wr, bool boolRead = true)
        {
            Re = new byte[] { 0 };
            if (!SerialPortObj.IsOpen) { return -4; }
            SerialPortObj.Write(Wr, 0, Wr.Length);
            Thread.Sleep(Wr.Length * 2);
            if (boolRead)
            {
                int length = SerialPortObj.BytesToRead;
                if (length < 1) { return -3; }
                if (length < 5) { SerialPortObj.Read(Re, 0, length); return -2; }
                Re = new byte[length];
                int d = SerialPortObj.Read(Re, 0, length);
                if (!CheckCRC(Re)) { return -1; }
                Thread.Sleep(length);
            }
            return 0;
        }
        public int ReadData(out byte[] Re)
        {
            Re = new byte[] { 0 };
            if (!SerialPortObj.IsOpen) { return -4; }
            int length = SerialPortObj.BytesToRead;
            if (length > 0)
            {
                Re = new byte[length];
                SerialPortObj.Read(Re, 0, length);
            }
            SerialPortObj.DiscardInBuffer();
            Thread.Sleep(10);
            return 0;
        }
        public bool CheckCRC(byte[] value)
        {
            var isOk = false;
            if (value != null && value.Length >= 2)
            {
                int length = value.Length;
                byte[] buf = new byte[length - 2];
                Array.Copy(value, 0, buf, 0, buf.Length);
                byte[] CRCbuf = Crc16(buf, buf.Length); //自己验证的结果
                if (CRCbuf[0] == value[length - 2] && CRCbuf[1] == value[length - 1]) { isOk = true; }//把上面验证的结果和串口返回的校验码（最后两个）进行比较
            }
            return isOk;
        }

        protected byte[] Crc16(byte[] pucFrame, int usLen)
        {
            int i = 0;
            byte[] res = new byte[2] { 0xFF, 0xFF };
            ushort iIndex;
            while (usLen-- > 0)
            {
                iIndex = (ushort)(res[0] ^ pucFrame[i++]);
                res[0] = (byte)(res[1] ^ aucCRCHi[iIndex]);
                res[1] = aucCRCLo[iIndex];
            }
            return res;
        }

        protected static readonly byte[] aucCRCHi = {
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
             0x00, 0xC1, 0x81, 0x40
         };

        protected static readonly byte[] aucCRCLo = {
             0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
             0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
             0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
             0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
             0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
             0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
             0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
             0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
             0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
             0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
             0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
             0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
             0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
             0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
             0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
             0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
             0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
             0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
             0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
             0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
             0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
             0x41, 0x81, 0x80, 0x40
         };
        public static byte[] CalculateCRC(byte[] pucFrame, int usLen)
        {
            int i = 0;
            byte[] res = new byte[2] { 0xFF, 0xFF };
            ushort iIndex;
            while (usLen-- > 0)
            {
                iIndex = (ushort)(res[0] ^ pucFrame[i++]);
                res[0] = (byte)(res[1] ^ aucCRCHi[iIndex]);
                res[1] = aucCRCLo[iIndex];
            }
            return res;
        }

        //public static byte[] CalculateCRC2(byte[] data, int len)
        //{
        //    //int len = data.Length;
        //    if (len > 0)
        //    {
        //        ushort crc = 0xFFFF;
        //        ushort poly = 0x1021;

        //        for (int i = 0; i < len; i++)
        //        {
        //            crc = (ushort)(crc ^ (data[i] << 8));
        //            for (int j = 0; j < 8; j++)
        //            {
        //                if ((crc & 0x8000)>0)
        //                {
        //                    crc = (ushort)((crc << 1) ^ poly);
        //                }
        //                else
        //                {
        //                    crc = (ushort)(crc << 1);
        //                }
        //                    //crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ poly) : (ushort)(crc >> 1);
        //            }
        //        }
        //        byte hi = (byte)((crc & 0xFF00) >> 8);  //高位置
        //        byte lo = (byte)(crc & 0x00FF);         //低位置

        //        return new byte[] { hi, lo };
        //    }
        //    return new byte[] { 0, 0 };
        //}
    }
}